/*=====================================================================*
 *                   Copyright (C) 2011 Paul Mineiro                   *
 * All rights reserved.                                                *
 *                                                                     *
 * Redistribution and use in source and binary forms, with             *
 * or without modification, are permitted provided that the            *
 * following conditions are met:                                       *
 *                                                                     *
 *     * Redistributions of source code must retain the                *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer.                                       *
 *                                                                     *
 *     * Redistributions in binary form must reproduce the             *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer in the documentation and/or            *
 *     other materials provided with the distribution.                 *
 *                                                                     *
 *     * Neither the name of Paul Mineiro nor the names                *
 *     of other contributors may be used to endorse or promote         *
 *     products derived from this software without specific            *
 *     prior written permission.                                       *
 *                                                                     *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND              *
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,         *
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES               *
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE             *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER               *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                 *
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES            *
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE           *
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR                *
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF          *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           *
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY              *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE             *
 * POSSIBILITY OF SUCH DAMAGE.                                         *
 *                                                                     *
 * Contact: Paul Mineiro <paul@mineiro.com>                            *
 *=====================================================================*/

#ifndef __DORMAND_PRINCE_HH__
#define __DORMAND_PRINCE_HH__

#include <algorithm>
#include <cmath>
#include <boost/optional.hpp>

namespace flassol
{
  // Solve { y'[x] == f[x, y[x]], y[x0] == y0 } return y[x1]

  template<typename F>
  boost::optional<float>
  dormand_prince_4_5 (const F&     f,
                      float        x0,
                      float        y0,
                      float        x1,
                      float        tol,
                      float        hmin,
                      float        hmax,
                      unsigned int maxevals)
    {
      boost::optional<float> rv;

      // Tableau from
      // http://en.wikipedia.org/wiki/Dormand-Prince_method

      float c2[] = { 1.0f / 5.0f, 1.0f / 5.0f };
      float c3[] = { 3.0f / 10.0f, 3.0f / 40.0f, 9.0f / 40.0f };
      float c4[] = { 4.0f / 5.0f, 44.0f / 45.0f, -56.0f / 15.0f, 32.0f / 9.0f };
      float c5[] = { 8.0f / 9.0f, 19372.0f / 6561.0f, -25360.0f / 2187.0f, 64448.0f / 6561.0f, -212.0f / 729.0f };
      float c6[] = { 1.0f, 9017.0f / 3168.0f, -355.0f / 33.0f, 46732.0f / 5247.0f, 49.0f / 176.0f, -5103.0f / 18656.0f };
      float c7[] = { 1.0f, 35.0f / 384.0f, 0.0f, 500.0f / 1113.0f, 125.0f / 192.0f, -2187.0f / 6784.0f,  11.0f / 84.0f };

      float b1[] = { 5179.0f / 57600.0f, 0.0f, 7571.0f / 16695.0f, 393.0f / 640.0f, -92097.0f / 339200.0f, 187.0f / 2100.0f, 1.0f / 40.0f };
      float *b2 = &c7[1];
      
      float x = x0;
      float y = y0;
      float h = std::min (hmax, x1 - x);

      for (unsigned int n = 0; n < maxevals && ! rv; n += 7)
        {
          float f1 = f (x, y);
          float f2 = f (x + c2[0] * h, y + h * (c2[1] * f1));
          float f3 = f (x + c3[0] * h, y + h * (c3[1] * f1 + c3[2] * f2));
          float f4 = f (x + c4[0] * h, y + h * (c4[1] * f1 + c4[2] * f2 + c4[3] * f3));
          float f5 = f (x + c5[0] * h, y + h * (c5[1] * f1 + c5[2] * f2 + c5[3] * f3 + c5[4] * f4));
          float f6 = f (x + c6[0] * h, y + h * (c6[1] * f1 + c6[2] * f2 + c6[3] * f3 + c6[4] * f4 + c6[5] * f5));
          float f7 = f (x + c7[0] * h, y + h * (c7[1] * f1 + c7[2] * f2 + c7[3] * f3 + c7[4] * f4 + c7[5] * f5 + c7[6] * f6));

          float error = fabsf ((b1[0] - b2[0]) * f1 +
                               // (b1[1] - b2[1]) * f2 +
                               (b1[2] - b2[2]) * f3 +
                               (b1[3] - b2[3]) * f4 +
                               (b1[4] - b2[4]) * f5 +
                               (b1[5] - b2[5]) * f6 +
                               b1[6] * f7);

          if (error < tol)
            {
              x = x + h;
              y = y + h * (b1[0] * f1 + 
                           // b1[1] * f2 +
                           b1[2] * f3 + 
                           b1[3] * f4 +
                           b1[4] * f5 +
                           b1[5] * f6 + 
                           b1[6] * f7);
            }

          if (x >= x1)
            {
              rv = y;
              break;
            }

          float delta = 0.84f * powf (tol / error, 1.0f / 5.0f);

          h = std::min (hmax, h * std::min (4.0f, std::max (delta, 0.1f)));

          if (h < hmin) // fail (!)
            {
              break;
            }

          h = std::min (h, x1 - x);
        }

      return rv;
    }
}

#endif // __DORMAND_PRINCE_HH__
